import numpy as np
from math import cos, sin, sqrt, atan2

from find_farthest_point_from_segment import find_farthest_point_from_segment_function


def Beisaier_path_planning_function(local_start_x, local_start_y, start_yaw,
                           end_x, end_y, end_yaw, curvature, forward_interval):
    """
    三阶贝塞尔曲线路径规划

    参数:
    local_start_x: 起始点x坐标
    local_start_y: 起始点y坐标
    start_yaw: 起始朝向角(弧度)
    end_x: 终点x坐标
    end_y: 终点y坐标
    end_yaw: 终点朝向角(弧度)
    curvature: 最大曲率约束
    forward_interval: 生成路径点之间的距离(米)

    返回:
    local_path_x: 路径x坐标列表
    local_path_y: 路径y坐标列表
    path_yaw: 路径点朝向角列表(弧度)
    mode: 空列表(不处理)
    lengths: 路径总长度
    """

    # 起始点P0
    P0 = np.array([local_start_x, local_start_y])

    # 终点P3
    P3 = np.array([end_x, end_y])

    # 计算起始方向向量
    start_dir = np.array([cos(start_yaw), sin(start_yaw)])

    # 计算终点方向向量
    end_dir = np.array([cos(end_yaw), sin(end_yaw)])

    # 计算起始点和终点之间的距离
    distance = np.linalg.norm(P3 - P0)

    # 根据最大曲率约束计算控制点距离
    control_distance = min(distance * 0.5, 1.0 / max(curvature, 1e-6))

    # 计算控制点P1 (沿起始方向)
    P1 = P0 + control_distance * start_dir

    # 计算控制点P2 (沿终点反方向)
    P2 = P3 - control_distance * end_dir

    # 计算路径总长度估计
    # 使用控制多边形长度作为估计
    total_length_estimate = (np.linalg.norm(P1 - P0) +
                             np.linalg.norm(P2 - P1) +
                             np.linalg.norm(P3 - P2))

    # 根据forward_interval计算需要的点数
    num_points = max(2, int(total_length_estimate / forward_interval) + 1)

    # 生成贝塞尔曲线参数t值
    t_values = np.linspace(0, 1, num_points)

    # 初始化输出列表
    local_path_x = []
    local_path_y = []
    path_yaw = []

    # 计算路径点
    for t in t_values:
        # 三阶贝塞尔曲线公式
        point = (1 - t) ** 3 * P0 + 3 * (1 - t) ** 2 * t * P1 + 3 * (1 - t) * t ** 2 * P2 + t ** 3 * P3
        local_path_x.append(point[0])
        local_path_y.append(point[1])

    # 计算每个路径点的朝向角(切线方向)
    for i in range(len(t_values)):
        t = t_values[i]
        if i < len(t_values) - 1:
            # 使用前向差分计算切线方向
            dx = local_path_x[i + 1] - local_path_x[i]
            dy = local_path_y[i + 1] - local_path_y[i]
        else:
            # 最后一个点使用后向差分
            dx = local_path_x[i] - local_path_x[i - 1]
            dy = local_path_y[i] - local_path_y[i - 1]

        # 计算朝向角，确保在[0, 2π)范围内
        yaw = atan2(dy, dx)
        if yaw < 0:
            yaw += 2 * np.pi
        path_yaw.append(yaw)

    # 计算路径总长度
    lengths = 0.0
    for i in range(1, len(local_path_x)):
        dx = local_path_x[i] - local_path_x[i - 1]
        dy = local_path_y[i] - local_path_y[i - 1]
        lengths += sqrt(dx * dx + dy * dy)

    Planned_OR_not = True

    return Planned_OR_not, local_path_x, local_path_y, path_yaw, lengths




def Calculating_minimum_TaoHang_Nummber(MIN_TURNING_RADIUS, TRACTOR_WIDTH, control_distance_limitation):
    start_x, start_y = 0, 0
    start_yaw = -np.pi / 2  # 朝东方向
    end_y = 0
    end_yaw = np.pi / 2  # 朝北方向
    max_curvature = 1/MIN_TURNING_RADIUS  # 最大曲率约束
    interval = 0.05  # 路径点间隔0.5米

    max_iterations = 0
    iteration = 1
    while TRACTOR_WIDTH*iteration < 3*MIN_TURNING_RADIUS:
        end_x = TRACTOR_WIDTH*iteration
        Planed_OR_not, local_path_x, local_path_y, path_yaw, total_length = Beisaier_path_planning_function(
            start_x, start_y, start_yaw, end_x, end_y, end_yaw, max_curvature, interval
        )
        if Planed_OR_not:
            farthest_point, max_distance, farthest_index = find_farthest_point_from_segment_function(local_path_x, local_path_y)
            max_width = max_distance
            if max_width<=control_distance_limitation:
                return iteration
        else:
            iteration += 1

    return 100000000





# 测试函数
if __name__ == "__main__":
    import matplotlib.pyplot as plt
    plt.rcParams['font.sans-serif'] = ['KaiTi']
    plt.rcParams['axes.unicode_minus'] = False

    MIN_TURNING_RADIUS = 8
    TRACTOR_WIDTH = 2
    control_distance_limitation = 10

    minimum_TaoHang_Nummber = Calculating_minimum_TaoHang_Nummber(MIN_TURNING_RADIUS, TRACTOR_WIDTH, control_distance_limitation)


    print("测试用例1：正常路径")
    start_x, start_y = 0, 0
    start_yaw = -np.pi / 2  # 朝东方向
    end_x, end_y = minimum_TaoHang_Nummber*TRACTOR_WIDTH, 0
    end_yaw = np.pi / 1.5  # 朝北方向
    max_curvature = 1/MIN_TURNING_RADIUS  # 最大曲率约束
    interval = 0.05  # 路径点间隔0.5米

    Planed_OR_not, local_path_x, local_path_y, path_yaw, lengths = Beisaier_path_planning_function(
        start_x, start_y, start_yaw, end_x, end_y, end_yaw, max_curvature, interval
    )

    plt.plot(local_path_x, local_path_y, 'b-', linewidth=2, label=f'曲率约束={max_curvature}')
    plt.plot([start_x], [start_y], 'ko', markersize=8, label='起点')
    plt.plot([end_x], [end_y], 'go', markersize=8, label='终点')
    plt.axis('equal')
    plt.grid(True)
    plt.legend()
    plt.title('路径')
    plt.xlabel('X坐标 (米)')
    plt.ylabel('Y坐标 (米)')
    plt.show()





    # 曲率分布
    # 计算曲率沿路径的变化
    curvatures = []
    distances = [0]
    for i in range(1, len(local_path_x)):
        dx = local_path_x[i] - local_path_x[i - 1]
        dy = local_path_y[i] - local_path_y[i - 1]
        distances.append(distances[-1] + sqrt(dx * dx + dy * dy))
        if i < len(local_path_x) - 1:
            # 近似计算曲率
            dx1 = local_path_x[i] - local_path_x[i - 1]
            dy1 = local_path_y[i] - local_path_y[i - 1]
            dx2 = local_path_x[i + 1] - local_path_x[i]
            dy2 = local_path_y[i + 1] - local_path_y[i]
            if sqrt(dx1 ** 2 + dy1 ** 2) > 1e-6 and sqrt(dx2 ** 2 + dy2 ** 2) > 1e-6:
                curvature_approx = 2 * fabs(dx1 * dy2 - dy1 * dx2) / ((dx1 ** 2 + dy1 ** 2) * (dx2 ** 2 + dy2 ** 2)) ** 0.5
                curvatures.append(curvature_approx)
            else:
                curvatures.append(0)
        else:
            curvatures.append(0)

    plt.plot(distances[1:], curvatures, 'b-', linewidth=2)
    plt.axhline(y=max_curvature, color='r', linestyle='--', label='最大允许曲率')
    plt.grid(True)
    plt.title('路径曲率分布')
    plt.xlabel('路径长度 (米)')
    plt.ylabel('曲率')
    plt.legend()
    plt.show()

    # 朝向角变化
    plt.plot(distances, path_yaw, 'b-', linewidth=2)
    plt.grid(True)
    plt.title('路径朝向角变化')
    plt.xlabel('路径长度 (米)')
    plt.ylabel('朝向角 (弧度)')
    plt.show()